# -*- coding: UTF-8 -d*-
import math

class Path:
    """
    路径规划类，实例化后可调用以下接口
    - distance_get(from, to) 获取从起点到终点的距离
    - ins_to_points(ins) 为输入的指令 ins 规划具体行车路经
    """
    def __init__(self, config=None):
        # 存放数据的文件夹
        data_folder = config.get('data_folder')
        self.dis_mart = self.get("%s/Distance Matrix" % data_folder)
        self.path_mart = self.get("%s/Path Matrix" % data_folder)
        self.point_arr = self.get_pointarr("%s/Co-ordinate" % data_folder)
        # 更新点坐标的时间间隔，单位（秒）
        self.point_interval = config.get('point_interval')

    def get(self, file_name):
        with open(file_name) as f1:
            list = f1.read()
            lines = list.split('\n')[:-1]
            mart = []
            for line in lines:
                m = line.split(' ')[:-1]
                mart.append([int(i) for i in m])
            return mart

    def get_pointarr(self, file_name):
        with open(file_name) as f1:
            list = f1.read()
            lines = list.split('\n')[:-1]
            mart = []
            for line in lines:
                m = {
                    "x": int(line.split(',')[0]),
                    "y": int(line.split(',')[1]),
                }
                mart.append(m)
            return mart

    def index_to_coord(self, index):
        return self.point_arr[index - 1]

    def path_get(self, start, target):
        path_list = []
        start -= 1
        target -= 1
        path_list.append(self.point_arr[start])
        temp = self.path_mart[start][target]
        while temp != target:
            path_list.append(self.point_arr[temp])
            temp = self.path_mart[temp][target]
        path_list.append(self.point_arr[target])
        return path_list

    def distance_get(self, start, target):
        return self.dis_mart[start - 1][target - 1]

    def culNextPoint(self, start, end, vel = 2):
        # 速度为0.1s运动的像素数量,暂定为 1
        x_dif = end['x'] - start['x']
        y_dif = end['y'] - start['y']
        ang = math.atan2(y_dif, x_dif)

        x_vel = math.cos(ang) * vel
        y_vel = math.sin(ang) * vel
        x_point = start['x']
        y_point = start['y']
        Pointarr_ms = []
        while (abs(x_point - end['x']) > abs(x_vel)) | (abs(y_point - end['y']) > abs(y_vel)):
            x_point += x_vel
            y_point += y_vel
            x = round(x_point)
            y = round(y_point)
            Pointarr_ms.append({"x": x, "y": y})
        Pointarr_ms.append({"x": end['x'], "y": end['y']})
        return Pointarr_ms

    def path_data(self, start, end, AGVid = 0):

        path = self.path_get(start, end)
        Pointarr = []

        for i in range(len(path) - 1):
            Pointarr += self.culNextPoint(path[i], path[i + 1])

        path_data = [{"Scheduledpath": {
            "AGVid": AGVid,
            "Pointarr": path,
            }
        }]
        return path_data, Pointarr
    
    def ins_to_points(self, instructions):
        '''
        将执行任务的指令通过路径规划翻译为点集 \n
        会对传入的指令列表中的每条指令添加以下键值
        - path   [路径上的打好的点（一组坐标）],
        - points [路径点（一组坐标）],
        - load   中间停车装货的点，points 的下标
        '''
        for ins in instructions:
            # 走第一段
            path, Pointarr = self.how_to_drive(ins.pos_start, ins.first_target)
            
            ins['points'] = Pointarr
            ins['path']   = path
            ins['load'] = len(Pointarr)

            # 如果有第二段的话
            if ins.second_target:
                path, Pointarr = self.how_to_drive(ins.first_target, ins.second_target)
                ins['points'] += Pointarr
                ins['path'] += path[1:]
    
    def departure_to_points(self, ins):
        """
        为发车（DepartureIns）指令规划路径，会在传入的指令对象里添加以下键值
        - path   [路径上的打好的点（一组坐标）],
        - points [路径点（一组坐标）]
        """
        path, Pointarr = self.how_to_drive(ins.origin, ins.destination)
        
        ins['points'] = Pointarr
        ins['path']   = path

    def how_to_drive(self, _from, _to):
        '''
        告知起点和终点，返回如何从起点走到终点的数据，例如路径点的坐标
        '''
        path = self.path_get(_from, _to)
        Pointarr = []
        for i in range(len(path) - 1):
            Pointarr += self.culNextPoint(path[i], path[i + 1])
        
        return path, Pointarr